import cv2
import math
from math import *
import test_calib
from pyzbar import pyzbar

def get_dist(a, b):
    return math.sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y))
def get_len(a, b, c):
    return math.sqrt(a*a+b*b*+c*c)
def get_avr_dist(a):
    sum = 0
    sum = sum + get_dist(a[0], a[1])
    sum = sum + get_dist(a[1], a[2])
    sum = sum + get_dist(a[2], a[3])
    sum = sum + get_dist(a[3], a[0])
    return sum/4
# 二维码动态识别
camera = cv2.VideoCapture(0)
mtx, roi, dist, newcameramtx = test_calib.camera_calib()
print(dist)
# camera.set(3, 1280)  # 设置分辨率
# camera.set(4, 768)
while True:
    (grabbed, frame) = camera.read()
    # 获取画面中心点
    h1, w1 = frame.shape[0], frame.shape[1]

    # 纠正畸变（这里把相机标定的代码去除了，各位自行标定吧）
    x,y,w,h = roi
    dst = cv2.undistort(frame, mtx, dist, None, newcameramtx)
    dst = dst[y:y+h, x:x+w]
    # 二值化
    # gray = cv2.cvtColor(dst, cv2.COLOR_RGB2GRAY)  #把输入图像灰度化
    # ret, binary = cv2.threshold(gray, 120, 255, cv2.THRESH_BINARY)
    # binary = dst
    # cv2.imshow("threshold", binary)
    # 扫描二维码
    text = pyzbar.decode(dst)
    for texts in text:
        textdate = texts.data.decode('utf-8')
        print(textdate)
        (x, y, w, h) = texts.rect  # 获取二维码的外接矩形顶点坐标
        print('二维码内容:' + textdate)

        # 二维码中心坐标
        cx = int(x + w / 2)
        cy = int(y + h / 2)
        cv2.circle(dst, (cx, cy), 2, (0, 255, 0), 8)  # 做出中心坐标
        # print('中间点坐标：', cx, cy)
        coordinate = (cx, cy)
        # 在画面左上角写出二维码中心位置
        cv2.putText(dst, 'QRcode_location' + str(coordinate), (20, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
        # 画出画面中心与二维码中心的连接线
        cv2.line(dst, (cx, cy), (int(w1 / 2), int(h1 / 2)), (255, 0, 0), 2)
        # cv2.rectangle(dst, (x, y), (x + w, y + h), (0, 255, 255), 2)  # 做出外接矩形
        # 二维码最小矩形
        cv2.line(dst, texts.polygon[0], texts.polygon[1], (255, 0, 0), 2)
        cv2.line(dst, texts.polygon[1], texts.polygon[2], (255, 0, 0), 2)
        cv2.line(dst, texts.polygon[2], texts.polygon[3], (255, 0, 0), 2)
        cv2.line(dst, texts.polygon[3], texts.polygon[0], (255, 0, 0), 2)
        
        z_dist=1000000/get_avr_dist(texts.polygon)/145/1.017
        x_dist=(cx-320)*1.37*z_dist/640
        y_dist=(cy-240)*1.15*z_dist/480
        code_dist = math.sqrt(z_dist*z_dist+x_dist*x_dist+y_dist*y_dist)
        # print(x_dist, y_dist, z_dist, z_dist*z_dist+x_dist*x_dist*+y_dist*y_dist)
        # print(code_dist)
        cv2.putText(dst, str(code_dist), (x - 10, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 50, 255), 2)

        # 写出扫描内容
        # txt = '(' + texts.type + ')  ' + textdate
        # cv2.putText(dst, txt, (x - 10, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 50, 255), 2)
        
    cv2.imshow('dst', dst)
    if cv2.waitKey(1) & 0xFF == ord('q'):  # 按q保存一张图片
        break
# 释放窗口
camera.release()
cv2.destroyAllWindows()